#include <kdl/frames.hpp>
#include <kdl/frames_io.hpp>
#include <math.h>

int main(int argc, char const *argv[])
{
    //Creating Vectors
    KDL::Vector v1;//Default constructor
    KDL::Vector v2(1.0,2.0,3.0);//Most used constructor
    KDL::Vector v3(v2);//Copy constructor
    KDL::Vector v4 = KDL::Vector::Zero();//Static member
    
    //Use operator << to print the values of your vector
    std::cout<<"v1 ="<<v1<<std::endl;
    std::cout<<"v2 = "<<v2<<std::endl;
    std::cout<<"v3 = "<<v3<<std::endl;
    std::cout<<"v4 = "<<v4<<std::endl;

    //Get/Set values of a vector
    v1[0]=4.0;
    v1[1]=5.0;
    v1[2]=6.0;

    //Creating a Rotation matrix from a rotation around a arbitrary
    //vector, the vector should not be normalised
    KDL::Rotation r8=KDL::Rotation::Rot(KDL::Vector(1.,2.,3.),M_PI/4);
    //Creating a Rotation matrix from a rotation around a arbitrary
    //vector, the vector should be normalised
    KDL::Rotation r9=KDL::Rotation::Rot2(KDL::Vector(0.4472,0.5477,0.7071),
                                         M_PI/4);

    // Printing the rotations:
    std::cout<<"r8: "<<r8<<std::endl;
    std::cout<<"r9: "<<r9<<std::endl;

    //The underlying unitvector X
    //r8.UnitX(v1);//or
    std::cout<<"UnitX of r8:"<<r8.UnitX()<<std::endl;
    //The underlying unitvector Y
    //r8.UnitY(v1);//or
    std::cout<<"Unity of r8:"<<r8.UnitY()<<std::endl;
    //The underlying unitvector Z
    //r8.UnitZ(v1);//or
    std::cout<<"UnitZ of r8:"<<r8.UnitZ()<<std::endl;

    return 0;
}
